
import processing.serial.*;
Serial port; 
Serial port2;

// y - 66537
// x - 101695
// definitions -------------------------------------
int LINE_FEED = '\r'; 

// variables ---------------------------------------
int validData = 0;
int timeout;
int timeoutCount = 0;
String inBuffer;
int xpos, ypos; 
int FOV, FOV0, FOV1;
int tl_x, tl_y, br_x, br_y;
//int stepWidth, stepHeight;
float columns, rows;
int processStep = 0;
boolean advance = true;
int delay_before_pic = 2000;
int delay_after_pic = 3000;
int m = 0;
int n = 0;
String x_string = "0";
String y_string = "0";


  int start_z = 0;
// 5x  int end_z = -40000;
// 5x  int step_z = -1000;

// 20x/0.35  int end_z = -5000;
// 20x/0.35  int step_z = -100;
  int end_z = -2000;
  int step_z = -50;
  int current_z = start_z;
  boolean done = false;

//byte[] rspCharArray = new byte[32];

/////////////////// setup //////////////////////////
void setup(){
  size(100,100);

  // List all the available serial ports:
  println(Serial.list());
  port = new Serial(this, Serial.list()[0], 9600);
  port2 = new Serial(this, Serial.list()[2], 9600);
  delay(500);
  port.clear();

  xpos = 0;
  ypos = 0;
  FOV = 800;
  tl_x = 0;
  tl_y = 0;
  br_x = 0;
  br_y = 0;
  //stepWidth=0;
  //stepHeight=0;
  


  rectMode(CENTER);
  strokeCap(SQUARE);
  noFill();
  smooth();
}


///////////////////// draw ///////////////////////////
void draw(){
  background(100,100,100);
  translate(1017,665);
  rotate(PI);


  
  if(done == false){
    println("current z = "+current_z+int(Float.parseFloat(commandReceive("pz\r"))) );
    println("done = "+done);
    if(current_z <= end_z) done = true;
    if(current_z == start_z) port.write("gz "+(current_z)+'\r');
      
    // take picture
    delay(delay_before_pic);
    port2.write("picture\r");
    delay(delay_before_pic);
      
    // increase current_z by step_z
    current_z = current_z + step_z;
    port.write("gz "+(current_z)+'\r');
         
  }
}






/////////////////// commandReceive() ///////////////////
public String commandReceive(String command) {
  // issue the command
  port.write(command);
  //delay(500);
  // get the response
  validData = 0;
  timeout=0;
  while(validData == 0){
    delay(100);
    timeout += 1;
    if(timeout > 4){
      println("timeout");
      validData = -1;
    }
  }
  if(validData != -1){    // we got a good response, now do something with it
    //println("inBuffer: "+inBuffer);
    return new String(inBuffer);
  }
  return null;
}



//////////////////// serialEvent ///////////////////
void serialEvent(Serial port) {
  inBuffer = port.readStringUntil(LINE_FEED);
  if (inBuffer != null) {
    validData = 1;
  }
}

/*
///////////////////// cross ////////////////////////
 void cross(int x, int y, int dim) {
 strokeWeight(5);
 line((x-(dim/2)), y, (x+(dim/2)), y);
 line(x, (y-(dim/2)), x, (y+(dim/2)));
 
 }
 *//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
